#include "Kdtree.h"
#include "Misc.h"
#include "Triangle.h"
#include <cassert>
#include <iostream>
#include "Object.h"

using namespace RayTrace;
CKdtree::CKdtree(void):mObject(NULL),root(NULL),mLeafFaceNum(10),mHeightOfTree(5)
{

}

CKdtree::~CKdtree(void)
{
}

void RayTrace::CKdtree::Init( Object * obj)
{
	mObject=obj;
	if (root)
	{
		root->destroy();
		delete root;
	}
	root =new node();
	//so object's bounding box must init before the kd-tree
	root->boundBox=obj->aabb;
	root->id=0;
	//faces will be overflow?
	root->faces.resize(mObject->Faces.size());
	for (unsigned short i=0;i<mObject->Faces.size();++i)
	{
		root->faces[i]=i;
	}
	root->left=root->right=NULL;
	CreateKdTree(root,mHeightOfTree);

}

void RayTrace::CKdtree::split( node * pNode)
{
	if (pNode)
	{
		switch(pNode->id)
		{
		case 0://x=c plane
			splitX(pNode,(pNode->boundBox.maxx+pNode->boundBox.minx)*0.5f);
			break;
		case 1://y=c plane
			splitY(pNode,(pNode->boundBox.maxy+pNode->boundBox.miny)*0.5f);
			break;
		case 2://z=c plane
			splitZ(pNode,(pNode->boundBox.maxz+pNode->boundBox.minz)*0.5f);
		    break;
		default:
			//this will never be happen
			assert(1);
		    break;
		}
	}

}

void RayTrace::CKdtree::splitX( node * pNode,float c )
{
	pNode->c=c;

	assert(pNode->left==NULL);
	assert(pNode->right==NULL);

	for (size_t i=0;i<pNode->faces.size();++i)
	{
		Face& face=mObject->Faces[pNode->faces[i]];
		CTriangle triangle=mObject->GetTriangleFromFace(face);
		bool insertLeft=false,insertRight=false;
		for (int j=0;j<3;++j)
		{
			if (insertLeft&&insertRight)
			{
				break;
			}
			if (triangle[j].x<=c && !insertLeft)
			{
				if (pNode->left==NULL)
				{
					pNode->left=new node();
					pNode->left->id=(pNode->id+1)%3;
					pNode->left->boundBox=pNode->boundBox;
					pNode->left->boundBox.maxx=c;
				}
				pNode->left->faces.push_back(pNode->faces[i]);
				insertLeft=true;
			}

			if (triangle[j].x>=c && !insertRight)
			{
				if (pNode->right==NULL)
				{
					pNode->right=new node();
					pNode->right->id=(pNode->id+1)%3;
					pNode->right->boundBox=pNode->boundBox;
					pNode->right->boundBox.minx=c;
				}
				pNode->right->faces.push_back(pNode->faces[i]);
				insertRight=true;
			}
		}
	}
	pNode->faces.clear();

}

void RayTrace::CKdtree::splitY( node * pNode,float c )
{
//	pNode->id=1;//y=c plane
	pNode->c=c;

	assert(pNode->left==NULL);
	assert(pNode->right==NULL);

	for (size_t i=0;i<pNode->faces.size();++i)
	{
		Face& face=mObject->Faces[pNode->faces[i]];
		CTriangle triangle=mObject->GetTriangleFromFace(face);
		bool insertLeft=false,insertRight=false;
		for (int j=0;j<3;++j)
		{
			if (insertLeft&&insertRight)
			{
				break;
			}
			if (triangle[j].y<=c && !insertLeft)
			{
				if (pNode->left==NULL)
				{
					pNode->left=new node();
					pNode->left->id=(pNode->id+1)%3;
					pNode->left->boundBox=pNode->boundBox;
					pNode->left->boundBox.maxy=c;
				}
				pNode->left->faces.push_back(pNode->faces[i]);
				insertLeft=true;
			}

			if (triangle[j].y>c && !insertRight)
			{
				if (pNode->right==NULL)
				{
					pNode->right=new node();
					pNode->right->id=(pNode->id+1)%3;
					pNode->right->boundBox=pNode->boundBox;
					pNode->right->boundBox.miny=c;
				}
				pNode->right->faces.push_back(pNode->faces[i]);
				insertRight=true;
			}
		}
	}
	pNode->faces.clear();
}

void RayTrace::CKdtree::splitZ( node * pNode,float c )
{
//	pNode->id=2;//y=c plane
	pNode->c=c;

	assert(pNode->left==NULL);
	assert(pNode->right==NULL);

	for (size_t i=0;i<pNode->faces.size();++i)
	{
		Face& face=mObject->Faces[pNode->faces[i]];
		CTriangle triangle=mObject->GetTriangleFromFace(face);
		bool insertLeft=false,insertRight=false;
		for (int j=0;j<3;++j)
		{
			if (insertLeft&&insertRight)
			{
				break;
			}
			if (triangle[j].z<=c && !insertLeft)
			{
				if (pNode->left==NULL)
				{
					pNode->left=new node();
					pNode->left->id=(pNode->id+1)%3;
					pNode->left->boundBox=pNode->boundBox;
					pNode->left->boundBox.maxz=c;
				}
				pNode->left->faces.push_back(pNode->faces[i]);
				insertLeft=true;
			}

			if (triangle[j].z>c && !insertRight)
			{
				if (pNode->right==NULL)
				{
					pNode->right=new node();
					pNode->right->id=(pNode->id+1)%3;
					pNode->right->boundBox=pNode->boundBox;
					pNode->right->boundBox.minz=c;
				}
				pNode->right->faces.push_back(pNode->faces[i]);
				insertRight=true;
			}
		}
	}
	pNode->faces.clear();
}

void RayTrace::CKdtree::CreateKdTree( node * r,int n)
{
	if (n==0)
	{
		return;
	}
	if (r!=NULL)
	{
		if (r->faces.size()>mLeafFaceNum)
		{
			split(r);
			if (r->left!=NULL)
			{
				CreateKdTree(r->left,n-1);
			}
			if (r->right!=NULL)
			{
				CreateKdTree(r->right,n-1);
			}
		}
	}
}

double  RayTrace::CKdtree::IntersectRay(const Ray& ray,int& faceindex)
{
	assert(root!=NULL);
	mCurRay=ray;
	return IntersectRayWithNode(ray,root,faceindex);
}

double RayTrace::CKdtree::IntersectRayWithNode( const Ray& ray,node * pNode,int &faceindex )
{
	if (pNode!=NULL)
	{
		//process intersection
		if (pNode->left==NULL && pNode->right==NULL)
		{
			bool intersect=false;
			double dist=MMAX;
			int fi;
			for (size_t i=0;i<pNode->faces.size();++i)
			{
				CTriangle triangle=mObject->GetTriangleFromFace(mObject->Faces[pNode->faces[i]]);
				if (triangle.IsThroughByRay(mCurRay,MMAX))
				{
					double tmpDist=triangle.DistRayThroughPlane(mCurRay);
					if (tmpDist>MMIN && tmpDist<dist)
					{
						dist=tmpDist;
						fi=pNode->faces[i];
						intersect=true;
					}
				}
			}
			if (intersect)
			{
				faceindex=fi;
				return dist;
			}
			else
			{
				return -1;
			}
		}
		int i=pNode->id;
		if ((ray.direction)[i]>MMIN)
		{
			if (ray.origin[i]>pNode->c)
			{
				return IntersectRayWithNode(ray,pNode->right,faceindex);
			}
			else
			{
				int fi;
				double t1=IntersectRayWithNode(ray,pNode->left,fi);
				if (t1>0)
				{
					faceindex=fi;
					return t1;
				}
				else
				{
					Ray tmpRay=ray;
					tmpRay.origin[i]=pNode->c;
					return IntersectRayWithNode(tmpRay,pNode->right,faceindex);
				}
			}
		}
		else if (ray.direction[i]<-1.0f*(MMIN))
		{
			if (ray.origin[i]<pNode->c)
			{
				return IntersectRayWithNode(ray,pNode->left,faceindex);
			}
			else
			{
				int fi;
				double t1=IntersectRayWithNode(ray,pNode->right,fi);
				if (t1>0)
				{
					faceindex=fi;
					return t1;
				}
				else
				{
					Ray tmpRay=ray;
					tmpRay.origin[i]=pNode->c;
					return IntersectRayWithNode(tmpRay,pNode->left,faceindex);
				}
			}
		}
		else//ray.direction[i]==(MMIN)OR-(MMIN)
		{
		
			if (ray.origin[i]<pNode->c)
			{
				return IntersectRayWithNode(ray,pNode->left,faceindex);
			}
			else
			{
				return IntersectRayWithNode(ray,pNode->right,faceindex);
			}
			
		/*	int fi,fj;
			double t1=IntersectRayWithNode(ray,pNode->left,fi);
			double t2=IntersectRayWithNode(ray,pNode->right,fj);

			if (t1<0 && t2<0)
			{
				return -1;
			}
			else if (t1<0 && t2>0)
			{
				faceindex=fj;
				return t2;
			}
			else if (t1>0 && t2<0)
			{
				faceindex=fi;
				return t1;
			}
			else
			{
				if (t1<t2)
				{
					faceindex=fi;
					return t1;
				}
				else
				{
					faceindex=fj;
					return t2;
				}
			}*/
		}
	}
	return -1;

}

